package com.paranoidmonkey.ode;

public class Quaternion {
	public float[] data;
	
	public Quaternion(){
		data=new float[4];
	}
	
	protected Quaternion(float[] d){
		data=d;
	}
	
	public Quaternion(float x,float y,float z,float w){
		data=new float[4];
		data[0]=w;
		data[1]=x;
		data[2]=y;
		data[3]=z;
	}

	public float w(){
		return data[0];
	}
	public float x(){
		return data[1];
	}
	public float y(){
		return data[2];
	}
	public float z(){
		return data[3];
	}
	
	public void setw(float val){
		data[0]=val;
	}
	public void setx(float val){
		data[1]=val;
	}
	public void sety(float val){
		data[2]=val;
	}
	public void setz(float val){
		data[3]=val;
	}

	public Matrix4 getMatrix(){
    float xx = x() * x();
    float xy = x() * y();
    float xz = x() * z();
    float xw = x() * w();
    float yy = y() * y();
    float yz = y() * z();
    float yw = y() * w();
    float zz = z() * z();
    float zw = z() * w();
 
		return new Matrix4(new float[]{
							1.0f - 2.0f * (yy + zz), 2.0f * (xy + zw), 2.0f * (xz - yw), 0.0f,
							2.0f * (xy - zw), 1.0f - 2.0f * (xx + zz), 2.0f * (yz + xw), 0.0f,
							2.0f * (xz + yw), 2.0f * (yz - xw), 1.0f - 2.0f * (xx + yy), 0.0f,
							0.0f, 0.0f, 0.0f, 1.0f });
	}
	
	public Quaternion getConjugate(){
		return new Quaternion(-x(), -y(), -z(), w());
	}
	
	public Quaternion multiply(Quaternion rhs){
		return new Quaternion((w() * rhs.x()) + (x() * rhs.w()) + (y() * rhs.z()) - (z() * rhs.y()),
		                  	   (w() * rhs.y()) + (y() * rhs.w()) + (z() * rhs.x()) - (x() * rhs.z()),
		                  	   (w() * rhs.z()) + (z() * rhs.w()) + (x() * rhs.y()) - (y() * rhs.x()),
		                  	   (w() * rhs.w()) - (x() * rhs.x()) - (y() * rhs.y()) - (z() * rhs.z()));
	}

	public static Quaternion slerp(Quaternion start,Quaternion end, float bias) {
        if (start.equals(end)) {
                return start;
        }

        float result = start.dot(end);

        if (result < 0.0) {
                // Negate the second quaternion and the result of the dot product
                end=end.multiply(-1);
                result = -result;
        }

        // Set the first and second scale for the interpolation
        float scale0 = 1 - bias;
        float scale1 = bias;

        // Check if the angle between the 2 quaternions was big enough to
        // warrant such calculations
        if ((1 - result) > 0.1) {// Get the angle between the 2 quaternions,
                // and then store the sin() of that angle
                final double theta = Math.acos(result);
                final double invSinTheta = 1f / Math.sin(theta);

                // Calculate the scale for q1 and q2, according to the angle and
                // it's sine value
                scale0 = (float)(Math.sin((1 - bias) * theta) * invSinTheta);
                scale1 = (float)(Math.sin((bias * theta)) * invSinTheta);
        }

        // Calculate the x, y, z and w values for the quaternion by using a
        // special form of linear interpolation for quaternions.
        return new Quaternion((scale0 * start.x()) + (scale1 * end.x()),
        					   (scale0 * start.y()) + (scale1 * end.y()),
        					   (scale0 * start.z()) + (scale1 * end.z()),
        					   (scale0 * start.w()) + (scale1 * end.w()));
	}

	public float dot(Quaternion rhs) {
        return (x() * rhs.x()) + (y() * rhs.y()) + (z() * rhs.z()) + (w() * rhs.w());
	}

	public Quaternion multiply(float scalar) {
        return new Quaternion(x()*scalar,
						       y()*scalar,
						       z()*scalar,
						       w()*scalar);
	}

}
